#include "config.h"
#ifdef RELEASE_ROBOT

#include <Servo.h> 
#include "motors.h"

#define SPEED 255
//servo
Servo wheelServo;
int servopos = 0;

double aright = 0;
double acenter = 0;
int dleft = 0;
int dcenter = 0;
int dright = 0;
int kdl = 0;
int kdc = 0;
int kdr = 0;
//time 
long time = 0;



// vars for logik
static int oldposition=0;
int valuem = SPEED;
int tikss = 0;
boolean label = false;
boolean label2 = false;
int timerr = 0;

//UART
char data; // this var is used for reading data from uart

void setup() { 

  analogReference(INTERNAL2V56); //analog init  

  motor_init();  // init pins and set motor to stop

  //button init (set pul-up resistor)
  pinMode(BUTTON, INPUT);
  digitalWrite(BUTTON,HIGH);

  // sensor digital init
  pinMode(SENSOR_DIGITAL_LEFT,INPUT);
  pinMode(SENSOR_LED_LEFT,OUTPUT);
  pinMode(SENSOR_DIGITAL_RIGHT,INPUT);
  pinMode(SENSOR_LED_RIGHT,OUTPUT);
  pinMode(SENSOR_DIGITAL_CENTER,INPUT);
  pinMode(SENSOR_LED_CENTER,OUTPUT);
  // initialize the LED pin as an output:
  pinMode(LED,OUTPUT);     

  // init servo and set it to midle
  wheelServo.attach(SERVO_PIN);  
  wheelServo.write(85);

  Serial1.begin(57600); // start UART
  Serial.begin(9600);

  forward(255);
  delay(1000);
  stopp();
  back(255);
  delay(1000);
  stopp();

  // say START
  delay(1000);  
  Serial1.print('S',BYTE);
  Serial1.println('1',BYTE);
  Serial.print('S',BYTE);
  Serial.println('0',BYTE);
}


void loop() 
{
  // start loop
  while(1)
  { 
    //timerr++;
    //move(valuem);
    //time = millis();
    // Get sensor information
    aright=acenter=0;  // set to zero
    dcenter = dleft = dright = 0; //set to zero
    for (int i = 0;i<5;i++) // cykle to repet 10 times 
    {
      dright += haveObstacle(SENSOR_LED_RIGHT,SENSOR_DIGITAL_RIGHT); // get value from digital right sensor
      aright += analogRead(DIST_SENSOR_RIGHT); // 
      dleft += haveObstacle(SENSOR_LED_LEFT,SENSOR_DIGITAL_LEFT);
      acenter += analogRead(DIST_SENSOR_CENTER);
      dcenter += haveObstacle(SENSOR_LED_CENTER,SENSOR_DIGITAL_CENTER);
    }
    aright = read_gp2d12_range(aright/5);
    acenter = read_gp2y0a_range(acenter/5);
    if(acenter>23) {   
      dcenter=dcenter<30?1:0;  
    }
    else  {   
      dcenter = 0;  
    }
    dleft=dleft<25?1:0;
    dright=dright<42?1:0;

    kdr=dright?kdr+1:0;
    kdl=dleft?kdl+1:0;
    kdc=dcenter?kdc+1:0;



    static int values=0;
    // valuem = acenter>80?80:acenter;
    //valuem = map(valuem,20,80,120,255);
    //valuem = acenter>300? -40:valuem;  
    values = aright>50?50:aright;
    values = aright<10?10:aright;
    values = map(values,10,50,-30,1);
    oldposition = writeServo(values);

    forward(255);
 

  }
  // end loop
}




// funktions for robots

// return value in sm it used for convert from right analod sharp sensor to sm
double read_gp2d12_range(double value)
{
  double val;
  val = (8762.88/(value/4 + 18.32)-12.06)/2;
  return val; 
}
// return value in sm it used for convert from center analod sharp sensor to sm
double read_gp2y0a_range(double value)
{
  double val;
  val = (8762.88/(value/4 + 18.32)-12.06);
  return val; 
} 


// this function return value (the distance between robot and wall if the distance is less than 5 it is a obstacle ) 
int haveObstacle(int ledPinInfra, int InputPin)
{
  static const int wait_time = 29;//28
  static int val = HIGH;
  static int number = 0;
  static const int ledPinRed = 13;
  val = HIGH;
  number = 0;
  for(int i=0;i<20;i++)//20
  {
    //digitalWrite(ledPinInfra, LOW);
    digitalWrite(ledPinInfra, HIGH);
    delayMicroseconds(wait_time);
    digitalWrite(ledPinInfra, LOW);
    delayMicroseconds(wait_time);

    val = digitalRead(InputPin);
    if (val == HIGH) {
      number++;
      digitalWrite(ledPinRed, LOW);
    } 
    else
      digitalWrite(ledPinRed, HIGH);
  }
  return number;
}


// this function check value it return the value in diapazon from -30 to +30  
int checkServo( int value)
{
  value = value>(30)?30:value;
  value = value<(-30)?-30:value;
  return value;
}
// depends of global class wheelServo 
// this function write value to servo and wait time in miliseconds
// this function use checkServo so the value could be any integer. 
int writeServoW (int value , int waitTime)
{
  if (wheelServo.attached() == false)
  {
    wheelServo.attach(SERVO_PIN);
  }
  value = checkServo(value);
  wheelServo.write(STCALC(value));
  delay(waitTime);
  wheelServo.detach();
  return value;
}
// depends of global class wheelServo 
// this function write value to servo without waiting time
// this function use checkServo so the value could be any integer. 
int writeServo (int value )
{
  if (wheelServo.attached() == false)
  {
    wheelServo.attach(SERVO_PIN);
  }
  value = checkServo(value);
  wheelServo.write(STCALC(value));
  return value;
}

// this function print the variables
void printVar()
{
  Serial.print((int)acenter);
  Serial.print("  ");
  Serial.print((int)aright);
  Serial.print("  ");
  Serial.print(dleft);
  Serial.print("  ");
  Serial.print(dcenter);
  Serial.print("  ");
  Serial.print(dright);
  Serial.print("  ");
  Serial.println(time);
  Serial.print(kdl);
  Serial.print("  ");
  Serial.print(kdc);
  Serial.print("  ");
  Serial.print(kdr);
  Serial.println("  ");
}


void move (int val)
{
  if (val>=0)
  {
    forward(val);
  }
  else
  {
    back(-val);
  }
}


#endif











